/*
 * Copyright (c) 2012 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */
#include <iostream>
#include "moveEmotion.h"

int checkAffectiveMove(AL::ALMotionProxy motion, int emotion, float intensity, int joint, int length){
	moveEmotion m1;
    m1.setDesiredJoints(joint);

    switch (emotion){
    case 1:
        m1.moveHappiness(motion,intensity,length);
        break;
    case 2:
        m1.moveSadness(motion,intensity,length);
        break;
    case 3:
        m1.moveAnger(motion,intensity,length);
        break;
    case 4:
        m1.moveSerenity(motion,intensity,length);
        break;
 
    }

    return 0;
}
int main(int argc, char* argv[])
{
    int emotion = 1;
    float intensity = 0.5f;

    int joint = 1;//Head only by default
    int length = 3;
      if(argc < 2)
      {
        std::cerr << "Wrong number of arguments!" << std::endl;
        std::cerr << "Usage: movehead NAO_IP [emotion] [intensity] [joint] [length]" << std::endl;
        std::cerr << "Usage: emotion   is in (1(happy), 2(sad), 3(angry), 4(serene))" << std::endl;
        std::cerr << "Usage: intensity is a float between [0.0, 1.0]" << std::endl;
        std::cerr << "Usage: joint     is an integer between [1, 7]" << std::endl;
        std::cerr << "Usage: length    is duration (in integer) of the expression in seconds" << std::endl;
        exit(2);
      }
      if (argc > 2){
          emotion = atoi(argv[2]);
      }

      if (argc > 3){
          intensity = atof(argv[3]);
      }
      if (argc > 4){
          joint = atoi(argv[4]);
      }
      if (argc > 5){
          length = atoi(argv[5]);
      }
      std::cout<<"Expression of ";
      switch (emotion){
      case 1:
          std::cout<<"Happiness ";
          break;
      case 2:
          std::cout<<"Sadness ";
          break;
      case 3:
          std::cout<<"Anger ";
          break;
      case 4:
          std::cout<<"Serenity ";
          break;
      }
      std::cout<<"with intensity="<<intensity<<" and joint="<<joint<<" and length="<<length<<" seconds"<<std::endl;

    
    char* naoIp=argv[1];
    /** Create a ALMotionProxy to call the methods to move NAO's head.
      * Arguments for the constructor are:
      * - IP adress of the robot
      * - port on which NAOqi is listening, by default 9559
      */
    AL::ALMotionProxy motion(naoIp, 9559);

    checkAffectiveMove(motion, emotion, intensity, joint, length);

    exit(0);
}
